#ifndef _math_quaternion_hpp_
#define _math_quaternion_hpp_

/** \file quaternion.hpp Supporting functionality for quaternions.
 *  \author Adrian Schweizer
 *  \created  $Do 30 Aug 07:27:48 pm CEST 2007 schwadri@SchwadriComp.local$
 *  \modified $So 28 Okt 06:02:25 pm CET 2007 schwadri@SchwadriLaptop$
 */

#include "matrix.hpp"
#include "vector.hpp"

namespace core {

    namespace math {

        /** \brief convert a \p vector<T,4> representing a quaternion to a matrix*/
        template<class T>
            matrix<T,3> quaternion_to_matrix(const vector<T,4>& q)
            {
                return matrix<T,3>(
                        1.0-2*(q[2]*q[2])-2*(q[3]*q[3]),
                            2*(q[1]*q[2])-2*(q[0]*q[3]),
                            2*(q[1]*q[3])+2*(q[0]*q[2]),

                            2*(q[1]*q[2])+2*(q[0]*q[3]),
                        1.0-2*(q[1]*q[1])-2*(q[3]*q[3]),
                            2*(q[2]*q[3])-2*(q[0]*q[1]),

                            2*(q[1]*q[3])-2*(q[0]*q[2]),
                            2*(q[2]*q[3])+2*(q[0]*q[1]),
                        1.0-2*(q[1]*q[1])-2*(q[2]*q[2]));
            }

        /** \brief convert a \p boost::math::quaternion to a matrix*/
        template<class T>
            matrix<T,3> quaternion_to_matrix(const boost::math::quaternion<T>& q)
            {
                return matrix<T,3>(
                        1.0-2*(q.R_component_3()*q.R_component_3())-2*(q.R_component_4()*q.R_component_4()),
                            2*(q.R_component_2()*q.R_component_3())-2*(q.R_component_1()*q.R_component_4()),
                            2*(q.R_component_2()*q.R_component_4())+2*(q.R_component_1()*q.R_component_3()),

                            2*(q.R_component_2()*q.R_component_3())+2*(q.R_component_1()*q.R_component_4()),
                        1.0-2*(q.R_component_2()*q.R_component_2())-2*(q.R_component_4()*q.R_component_4()),
                            2*(q.R_component_3()*q.R_component_4())-2*(q.R_component_1()*q.R_component_2()),

                            2*(q.R_component_2()*q.R_component_4())-2*(q.R_component_1()*q.R_component_3()),
                            2*(q.R_component_3()*q.R_component_4())+2*(q.R_component_1()*q.R_component_2()),
                        1.0-2*(q.R_component_2()*q.R_component_2())-2*(q.R_component_3()*q.R_component_3()));
            }

    } // namespace math

} // namespace core

#endif // _math_quaternion_hpp_
